
#include <rtthread.h>
#include "fc_error.h"
#include "usercmd.h"
#include "fc_sensors.h"
#include "zino.h"
#include "rc.h"
#include "program.h"
#define DBG_TAG "program"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#define POS_FIX_X_Y(X) (0.01 * (X))
#define MAX_FLY_HEIGHT_LIMIT 300.0 // cm

#define PRO_EVENT_STATE_CHECK(x) (pro_event_state & (x))
#define PRO_EVENT_STATE_SET(x) (pro_event_state |= (x))
#define PRO_EVENT_STATE_RESET(x) (pro_event_state &= ~(x))

bool pro_autotakingoff = false;
bool pro_autolanding = false;
rt_event_t pro_event;
static uint32_t pro_event_state = 0;
static bool fc_main_disable = false;
int16_t mixly_pitch_angel = 0;
int16_t mixly_roll_angel = 0;
int16_t mixly_yaw_rate = 0;
void pro_disable_fc()
{
    fc_main_disable = true;
}

// 函数：pro_enable_fc
// 功能：启用飞行控制
// 函数：pro_enable_fc
// 功能：启用飞行控制
// 将飞行控制禁用标志设置为false，即启用飞行控制
void pro_enable_fc()
{
    fc_main_disable = false;
}
bool get_fc_main_state()
{
    return fc_main_disable;
}

void pro_event_state_set(uint32_t eventstate)
{
    PRO_EVENT_STATE_SET(eventstate);
}
void pro_event_state_reset(uint32_t eventstate)
{
    PRO_EVENT_STATE_RESET(eventstate);
}

void pro_event_send(uint32_t event)
{
    if (FC_ERROR_CHECK(FC_PROGRAM_MODE) && FC_ERROR_CHECK(FC_PRORAM_READY))
    {
        rt_event_send(pro_event, event);
    }
}

bool check_pro_event_enable()
{
    return PRO_EVENT_STATE_CHECK(PRO_EVENT_ENABLE);
}

void pro_event_send_enable()
{
    PRO_EVENT_STATE_SET(PRO_EVENT_ENABLE);
}

void pro_event_reset_all()
{

    PRO_EVENT_STATE_RESET(PRO_EVENT_START);
    PRO_EVENT_STATE_RESET(PRO_EVENT_AIRBORNE);
    PRO_EVENT_STATE_RESET(PRO_EVENT_TAKEOFF);
    PRO_EVENT_STATE_RESET(PRO_EVENT_LAND);
    PRO_EVENT_STATE_RESET(PRO_EVENT_LANDED);
}

void pro_event_start()
{
    if (!PRO_EVENT_STATE_CHECK(PRO_EVENT_START))
    {
        PRO_EVENT_STATE_SET(PRO_EVENT_START);
        pro_event_send(PRO_EVENT_START);
        // LOG_D("set start\n");
    }
}

void pro_drone_ctrl(int16_t pitch, int16_t roll, int16_t throttle, int16_t yaw)
{

    // LOG_D("pitch %d roll %d throttle %d yaw %d FC_AIRBORN:%d\n", pitch, roll, throttle, yaw, FC_ERROR_CHECK(FC_AIRBORN));
    if (!FC_ERROR_CHECK(FC_AIRBORN))
        return;
    // NOTE 在空中了
    static uint8_t ctrlState = 0;
    if (pitch)
    {
        setPoint.Pos.x = zdrone_state.pos.x + POS_FIX_X_Y(pitch);
        ctrlState |= 0x01;
    }
    if (roll)
    {
        setPoint.Pos.y = zdrone_state.pos.y + POS_FIX_X_Y(roll);
        ctrlState |= 0x02;
    }
    if (throttle)
    {
        setPoint.Pos.z = zdrone_state.pos.z + throttle;
        ctrlState |= 0x04;
    }
    if (yaw)
    {
        if (yaw > 180)
            yaw = 180;
        if (yaw < -180)
            yaw = -180;
        setPoint.Angle.yaw = zdrone_state.attitude.yaw + yaw;
        ctrlState |= 0x08;
    }
    while (ctrlState != 0) // 等待所有机动完成
    {
        if (!FC_ERROR_CHECK(FC_PROGRAM_MODE))
        {
            pro_event_reset_all();
            ctrlState = 0;
            return;
        }
        if (pitch)
        {
            if (ABS(zdrone_state.pos.x - setPoint.Pos.x) < 0.05f)
            {
                ctrlState &= ~0x01;
                setPoint.manual[X] = 0;
                // LOG_D("cmd pitch done\n");
            }
        }

        if (roll)
        {
            if (ABS(zdrone_state.pos.y - setPoint.Pos.y) < 0.05f)
            {
                ctrlState &= ~0x02;
            }
        }

        if (throttle)
        {

            if (ABS(zdrone_state.pos.z - setPoint.Pos.z) <= 5.0f)
            {
                ctrlState &= ~0x04;
            }
        }

        if (yaw)
        {
            if (ABS(zdrone_state.attitude.yaw - setPoint.Angle.yaw) < 5.0f)
            {
                ctrlState &= ~0x08;
            }
        }

        rt_thread_mdelay(10);
    }
}

void pro_drone_takeoff()
{
    // if (!FC_ERROR_CHECK(FC_PROGRAM_MODE))
    //     return;
    // if (FC_ERROR_CHECK(FC_ARMED))
    //     return;
    // if (PRO_EVENT_STATE_CHECK(PRO_EVENT_AIRBORNE))
    //     return;
    // if (!PRO_EVENT_STATE_CHECK(PRO_EVENT_TAKEOFF))
    // {
    //     PRO_EVENT_STATE_SET(PRO_EVENT_TAKEOFF);
    //     if (!FC_ERROR_CHECK(FC_ARMED))
    //     {
    //         PRO_EVENT_STATE_RESET(PRO_EVENT_LANDED);
    //         FC_ERROR_SET(FC_ARMED);
    //         pro_autotakingoff = true;
    //     }
    // }
    if (FC_ERROR_CHECK(FC_AIRBORN) || FC_ERROR_CHECK(FC_LANDING) || FC_ERROR_CHECK(FC_TAKINGOFF))
        return;
    FC_ERROR_SET(FC_ARMING_ALERK);
    // FC_ERROR_SET(FC_TAKINGOFF);
    FC_ERROR_SET(FC_AUTO_TAKINGOFF);
    // LOG_D("takingoff\n");
    // NOTE "RT_WAITING_FOREVER" 会一直等，直到收到 set “PRO_EVENT_AIRBORNE”
    uint32_t event = 0;
    if (rt_event_recv(pro_event, PRO_EVENT_AIRBORNE | PRO_EVENT_STOP, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER, &event) == RT_EOK)
    {
        // if (event & PRO_EVENT_STOP)
        // {
        //     FC_ERROR_RESET(FC_PROGRAM_MODE);
        //     pro_event_reset_all();
        //     return;
        // }
        // PRO_EVENT_STATE_RESET(PRO_EVENT_TAKEOFF);
        // NOTE 起飞完成后，pro_event_send(PRO_EVENT_AIRBORNE);
        LOG_D("takeoff done, airborne!");
    }
}

void pro_drone_land()
{
    // if (!FC_ERROR_CHECK(FC_PROGRAM_MODE))
    //     return;
    // if (!FC_ERROR_CHECK(FC_ARMED))
    //     return;
    // if (PRO_EVENT_STATE_CHECK(PRO_EVENT_LANDED))
    //     return;
    // if (PRO_EVENT_STATE_CHECK(PRO_EVENT_AIRBORNE))
    // {
    //     PRO_EVENT_STATE_SET(PRO_EVENT_LAND);
    //     pro_autolanding = true;
    // }
    // NOTE "RT_WAITING_FOREVER" 会一直等，直到收到 set “PRO_EVENT_AIRBORNE”
    // if(FC_ERROR_CHECK(FC_LANDING) || FC_ERROR_CHECK(FC_TAKINGOFF) || !FC_ERROR_CHECK(FC_ARMED) || )
    if (!FC_ERROR_CHECK(FC_AIRBORN))
        return;

    LOG_D("landing\n");
    FC_ERROR_SET(FC_LANDING);
    FC_ERROR_RESET(FC_AIRBORN);
    uint32_t event = 0;
    if (rt_event_recv(pro_event, PRO_EVENT_LANDED | PRO_EVENT_STOP, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER, &event) == RT_EOK)
    {
        // if (event & PRO_EVENT_STOP)
        // {
        //     FC_ERROR_RESET(FC_PROGRAM_MODE);
        //     pro_event_reset_all();
        //     return;
        // }
        // PRO_EVENT_STATE_RESET(PRO_EVENT_LAND);
        // PRO_EVENT_STATE_RESET(PRO_EVENT_AIRBORNE);
        // FC_ERROR_RESET(FC_PROGRAM_MODE);
        // NOTE 起飞完成后，pro_event_send(PRO_EVENT_AIRBORNE);
        // LOG_D("land done, landed!");
        // fuck
    }
}

void pro_drone_stop()
{
    if (FC_ERROR_CHECK(FC_PROGRAM_MODE))
    {
        if (PRO_EVENT_STATE_CHECK(PRO_EVENT_ENABLE))
        {
            PRO_EVENT_STATE_RESET(PRO_EVENT_ENABLE);
            pro_event_reset_all();
            FC_ERROR_RESET(FC_PROGRAM_MODE);
        }
    }
}

int16_t get_rc_data(uint8_t ch)
{
    if (ch < 8)
        return get_rc_channel_ram()->rx_msg.v.channel[ch];
    else
        return 0;
}

void set_rgb_color(uint8_t num, uint8_t r, uint8_t g, uint8_t b)
{
    // rt_kprintf("set rgb color %d %d %d\n", r, g, b);
    if (r > 255)
        r = 255;
    if (g > 255)
        g = 255;
    if (b > 255)
        b = 255;
    if (r < 0)
        r = 0;
    if (g < 0)
        g = 0;
    if (b < 0)
        b = 0;
    rgb_set_color(num, r, g, b);
    rgb_show();
}

void set_all_rgb_color(uint8_t r, uint8_t g, uint8_t b)
{
    // rt_kprintf("set rgb color %d %d %d\n", r, g, b);
    if (r > 255)
        r = 255;
    if (g > 255)
        g = 255;
    if (b > 255)
        b = 255;
    if (r < 0)
        r = 0;
    if (g < 0)
        g = 0;
    if (b < 0)
        b = 0;
    rgb_set_color(0, r, g, b);
    rgb_set_color(1, r, g, b);
    rgb_set_color(2, r, g, b);
    rgb_set_color(3, r, g, b);
    rgb_set_color(4, r, g, b);
    rgb_show();
}

void reset_rgb_color(uint8_t num, uint8_t r, uint8_t g, uint8_t b)
{
    rgb_set_color(num, 0, 0, 0);
    rgb_show();
}

void set_motor_speed(uint8_t num, int16_t speed)
{
    motor_on_all();
    if (speed > 1000)
        speed = 1000;
    motor_set_duty(num, speed);
}

void set_motor_speed_all(int16_t speed)
{
    if (speed > 1000)
        speed = 1000;
    motor_on_all();
    motor_set_duty(1, speed);
    motor_set_duty(2, speed);
    motor_set_duty(3, speed);
    motor_set_duty(4, speed);
}

void reset_all_motor()
{
    motor_off_all();
}

void pro_drone_unlock(int input)
{
    if (input == 0)
    {
        FC_ERROR_SET(FC_ARMED);
    }
    else
    {
        FC_ERROR_RESET(FC_ARMED);
    }
}
void pro_drone_headmode(int input)
{
    if (input == 0)
    {
        FC_ERROR_RESET(FC_HEADLESS_MODE);
    }
    else
    {
        FC_ERROR_SET(FC_HEADLESS_MODE);
    }
}
void pro_drone_baroMode(int input)
{
    if (input == 0)
    {
        FC_ERROR_RESET(FC_MANUAL_MODE);
    }
    else
    {
        FC_ERROR_SET(FC_MANUAL_MODE);
    }
}
void pro_drone_flipMode(int input)
{
    if (!FC_ERROR_CHECK(FC_AIRBORN))
        return;
    // LOG_D("flip mode %d\n", input);
    fc_flip_ctrl(input);
    uint32_t event = 0;
    if (rt_event_recv(pro_event, PRO_EVENT_FLIP_DONE | PRO_EVENT_STOP, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER, &event) == RT_EOK)
    {
    }
}
// void pro_drone_pitch_angle(int input)
// {
//     setPoint.Angle.pitch = input;
// }
// void pro_drone_roll_angle(int input)
// {
//     setPoint.Angle.roll = input;
// }
// void pro_drone_yaw_rate(int input)
// {
//     setPoint.AngleRate.yaw = input;
// }
int is_rc_key_down(int input)
{
    if (MIXLY_RC_RT == input)
    {
        return ((get_rc_channel(RC_KEY_VALUE) & (1 << BALL_KEY_RT)) != 0);
    }
    else if (MIXLY_RC_LT == input)
    {
        return ((get_rc_channel(RC_KEY_VALUE) & (1 << BALL_KEY_LT)) != 0);
    }

    return 0;
}
int ge_rc_value(int input)
{
    if (input == MIXLY_RC_RLR)
    {
        return get_rc_channel(RC_RLR);
    }
    else if (input == MIXLY_RC_RUD)
    {
        return get_rc_channel(RC_RUD);
    }
    else if (input == MIXLY_RC_LLR)
    {
        return get_rc_channel(RC_LLR);
    }
    else if (input == MIXLY_RC_LUD)
    {
        return get_rc_channel(RC_LUD);
    }
    return 0;
}
